#include <manual/input.h>


/**
 *  Input constructor.
 **/
Input::Input ()
{
	ros::NodeHandle n;
	move_publisher = n.advertise<driver::Move>("motor_controller/directions", 1);
	joy_subscriber = n.subscribe<sensor_msgs::Joy>("joy", 10, & Input::joystick_callback, this);

	signal(SIGINT, Input::quit);
	boost::thread my_thread(boost::bind(&Input::keyboard_loop, this));

	ROS_INFO("Input ready.\n");  
	
	ros::spin();
	
	my_thread.interrupt() ;
	my_thread.join() ;	
}


/**
 * 	Input publish. This creates and sends movement commands (in
 *	the form of Move messages) to the move_publisher topic. 
 * 
 * 	Expects a distance (positive) and an angle (clockwise negative,
 * 	counterclockwise positive, in radians).
 **/
void Input::publish (double distance, double angle)
{
	driver::Move move;
	move.distance 	= distance;
	move.angle 	= angle;
	move.wait_until_completion = false;
	move_publisher.publish(move);
}


/**
 * 	Manual Input
 * 	
 * 	This node provides manual input to motor controllers. Manual input
 * 	may originate from a keyboard, joystick, or other device.
 */
int main (int argc, char **argv)
{
  	ros::init(argc, argv, "controller_manual");
	new Input();
	return 0;
}

